#include "simulate.h"
#include <cmath>
#include <iostream>

using namespace std;

Simulator::Simulator(float step){
     shapes = vector<RigidBody*>();
     h = step;                  
}

void Simulator::renderShapes(){
     int i;
     for(i = 0; i < shapes.size(); i++){
           shapes[i]->renderSelf();                 
     }
}

void Simulator::addShape(RigidBody* shape){
     shapes.push_back(shape);
}



void Simulator::updateStates()
{
    if(areColliding(shapes[0]->position,shapes[0]->size2,shapes[1]->position,shapes[1]->size2))
    {
        cout<<"collision"<<endl;
        /*MyVec3 theCube=shapes[0]->linMomentum;
        MyVec3 theSphere=shapes[1]->linMomentum;
        float bounceMag=1;
        
        //shapes[0]->linMomentum.n[0]*=-1;
        theCube.n[0]*=bounceMag;
        theCube.n[1]*=bounceMag;
        theCube.n[2]*=bounceMag;
        
        theCube.n[0]*=bounceMag;
        theCube.n[1]*=bounceMag;
        theCube.n[2]*=bounceMag;
        shapes[0]->linMomentum=theCube;
        shapes[1]->linMomentum=theSphere;
        */
        applyColForce();
    }




    int i;
    applyGravityForce();
    
    for(i = 0; i < shapes.size(); i++)
    {
        if(shapes[i]->position.n[1] <= -5)
        {
            shapes[i]->linMomentum.n[1]*=-.8;//adding bounce = 0; 
                       
        }
        applyDragForce();
        shapes[i]->ODEify(h);
        shapes[i]->clearForces();        
    }    
   
    
}

void Simulator::applyGravityForce(){
    int i;
    for(i = 0; i < shapes.size(); i++){
        shapes[i]->forceTotal = shapes[i]->forceTotal + MyVec3(0,-19.0,0);
    }
}

void Simulator::applyUserForce(MyVec3 force){
    shapes[0]->forceTotal = shapes[0]->forceTotal + force;
}
void Simulator::applyColForce()
{

float mag=.3;
    /*shapes[0]->forceTotal.n[0]+=shapes[1]->forceTotal.n[0]*mag;
    shapes[0]->forceTotal.n[1]+=shapes[1]->forceTotal.n[1]*mag;
    shapes[0]->forceTotal.n[2]+=shapes[1]->forceTotal.n[2]*mag;
    
    shapes[1]->forceTotal.n[0]+=shapes[0]->forceTotal.n[0]*mag;
    shapes[1]->forceTotal.n[1]+=shapes[0]->forceTotal.n[1]*mag;
    shapes[1]->forceTotal.n[2]+=shapes[0]->forceTotal.n[2]*mag;*/
    
    shapes[0]->linMomentum.n[0] = (shapes[1]->forceTotal.n[0]*mag);
    shapes[0]->linMomentum.n[1] = (shapes[1]->forceTotal.n[1]*mag);
    shapes[0]->linMomentum.n[2] = (shapes[1]->forceTotal.n[2]*mag);
    
    shapes[1]->linMomentum.n[0] = (shapes[0]->forceTotal.n[0]*mag);
    shapes[1]->linMomentum.n[1] = (shapes[0]->forceTotal.n[1]*mag);
    shapes[1]->linMomentum.n[2] = (shapes[0]->forceTotal.n[2]*mag);
    /*shapes[0]->forceTotal.n[0] = (shapes[1]->linMomentum.n[0]);
    shapes[0]->forceTotal.n[1] = (shapes[1]->linMomentum.n[1]);
    shapes[0]->forceTotal.n[2] = (shapes[1]->linMomentum.n[2]);
    
    shapes[1]->forceTotal.n[0] = (shapes[0]->linMomentum.n[0]);
    shapes[1]->forceTotal.n[1] = (shapes[0]->linMomentum.n[1]);
    shapes[1]->forceTotal.n[2] = (shapes[0]->linMomentum.n[2]);*/

/*    shapes[0]->forceTotal.n[0] = shapes[0]->forceTotal.n[0] + (shapes[1]->linMomentum.n[0]);
    shapes[0]->forceTotal.n[1] = shapes[0]->forceTotal.n[1] + (shapes[1]->linMomentum.n[1]);
    shapes[0]->forceTotal.n[2] = shapes[0]->forceTotal.n[2] + (shapes[1]->linMomentum.n[2]);
    
    shapes[1]->forceTotal.n[0] = shapes[1]->forceTotal.n[0] + (shapes[0]->linMomentum.n[0]);
    shapes[1]->forceTotal.n[1] = shapes[1]->forceTotal.n[1] + (shapes[0]->linMomentum.n[1]);
    shapes[1]->forceTotal.n[2] = shapes[1]->forceTotal.n[2] + (shapes[0]->linMomentum.n[2]);*/
    
    
}

void Simulator::applyDragForce(){

float mag=-.9;
    shapes[0]->forceTotal.n[0]+=shapes[0]->forceTotal.n[0]*mag;
    shapes[0]->forceTotal.n[1]+=shapes[0]->forceTotal.n[1]*mag;
    shapes[0]->forceTotal.n[2]+=shapes[0]->forceTotal.n[2]*mag;
    
    shapes[1]->forceTotal.n[0]+=shapes[1]->forceTotal.n[0]*mag;
    shapes[1]->forceTotal.n[1]+=shapes[1]->forceTotal.n[1]*mag;
    shapes[1]->forceTotal.n[2]+=shapes[1]->forceTotal.n[2]*mag;
    
}
